#pragma once

#include "cartographer/rcm/imu/integrate.h"
#include <sophus/se3.hpp>

namespace cartographer
{
    // IMU calibration (Tbc, Tcb, noise)
    class ImuCalib
    {
    public:
        ImuCalib(const Sophus::SE3<float> &Tbc, const float &ng, const float &na, const float &ngw, const float &naw)
        {
            Set(Tbc, ng, na, ngw, naw);
        }

        ImuCalib(const ImuCalib &calib);

        ImuCalib()
        {
            mbIsSet = false;
        }

        // void Set(const cv::Mat &cvTbc, const float &ng, const float &na, const float &ngw, const float &naw);
        void Set(const Sophus::SE3<float> &sophTbc, const float &ng, const float &na, const float &ngw, const float &naw);

    public:
        // Sophus/Eigen implementation
        Sophus::SE3<float> mTcb;
        Sophus::SE3<float> mTbc;
        Eigen::DiagonalMatrix<float, 6> Cov, CovWalk;
        bool mbIsSet;
    };

    struct ImuData
    {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        ImuData()
        {
        }
        float time;
        Eigen::Vector3f accelerate;
        Eigen::Vector3f angular_velocity;
    };

    class ImuContainer
    {
    public:
        ImuContainer()
        {
        }
    };

    ImuCalib GetImuCalib();

    // Integration of 1 gyro measurement
    class IntegratedRotation
    {
    public:
        IntegratedRotation()
        {
        }
        IntegratedRotation(const Eigen::Vector3f &angVel, const ImuBias &imuBias, const float &time);

    public:
        float deltaT; // integration time
        Eigen::Matrix3f deltaR;
        Eigen::Matrix3f rightJ; // right jacobian
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    class ImuPreintegrate
    {
    public:
        using Ptr = std::shared_ptr<ImuPreintegrate>;

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW ImuPreintegrate(const ImuBias &b_);

        ImuPreintegrate()
        {
        }

        virtual ~ImuPreintegrate()
        {
        }

        void Initialize(const ImuBias &b_);

        void Update(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt);

        ImuBias GetDeltaBias(const ImuBias &b_);

        void SetNewBias(const ImuBias &bu_);

        Eigen::Matrix3f GetDeltaRotation(const ImuBias &b_);
        Eigen::Vector3f GetDeltaVelocity(const ImuBias &b_);
        Eigen::Vector3f GetDeltaPosition(const ImuBias &b_);

        Eigen::Matrix3f GetUpdatedDeltaRotation();
        Eigen::Vector3f GetUpdatedDeltaVelocity();
        Eigen::Vector3f GetUpdatedDeltaPosition();

        Eigen::Matrix3f GetOriginalDeltaRotation();
        Eigen::Vector3f GetOriginalDeltaVelocity();
        Eigen::Vector3f GetOriginalDeltaPosition();

        Eigen::Matrix<float, 6, 1> GetDeltaBias();

        ImuBias GetOriginalBias();

        ImuBias GetUpdatedBias();

    public:
        float dT;
        Eigen::Matrix<float, 15, 15> C;
        Eigen::Matrix<float, 15, 15> Info;
        Eigen::DiagonalMatrix<float, 6> Nga, NgaWalk;

        // Values for the original bias (when integration was computed)
        ImuBias b;
        Eigen::Matrix3f dR;
        Eigen::Vector3f dV, dP;
        Eigen::Vector3f avgA, avgW;
        Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;

    private:
        std::mutex mMutex;
        // Updated bias
        ImuBias bu;
        // Dif between original and updated bias
        // This is used to compute the updated values of the preintegration
        Eigen::Matrix<float, 6, 1> db;

        struct integrable
        {
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW integrable()
            {
            }
            integrable(const Eigen::Vector3f &a_, const Eigen::Vector3f &w_, const float &t_)
                : a(a_), w(w_), t(t_)
            {
            }
            Eigen::Vector3f a, w;
            float t;
        };

        std::vector<integrable> mvMeasurements;
    };

    int RunImuPreintegrate(ImuPreintegrate::Ptr &integrator,
                           const std::deque<sensor::ImuData> imu_data,
                           const common::Time &time_start,
                           const common::Time &time_end);
} // namespace cartographer